// Fill out your copyright notice in the Description page of Project Settings.

#include "MPU6050Test.h"
#include "ActorPose.h"

ActorPose::ActorPose()
{
}

ActorPose::~ActorPose()
{
}

void ActorPose::InputRotation(float roll, float pitch, float yaw)
{
	Poco::ScopedLock<Poco::Mutex>	lock(Mutex);

	CurrentRotation = FRotator(-pitch, -yaw, roll);
}

void ActorPose::InputAcceleration(float x, float y, float z)
{
	/*
	float fRadAlpha = FMath::DegreesToRadians(CurrentRotation.Roll);
	float fRadBeta = FMath::DegreesToRadians(-CurrentRotation.Pitch);
	float fRadGamma = FMath::DegreesToRadians(-CurrentRotation.Yaw);

	float Gx = -1.0f * FMath::Sin(fRadBeta);
	float Gy = 1.0f * FMath::Sin(fRadAlpha);
	UE_LOG(LogClass, Log, TEXT("Gx: %f, Gy: %f"), Gx, Gy);

	FVector vecLocal(x - Gx, y - Gy, 0);

	FVector vecX, vecY, vecZ, vecW;

	vecX.X = FMath::Cos(fRadBeta) * FMath::Cos(fRadGamma);
	vecX.Y = -FMath::Cos(fRadAlpha) * FMath::Sin(fRadGamma) + FMath::Sin(fRadAlpha) * FMath::Sin(fRadBeta) * FMath::Cos(fRadGamma);
	vecX.Z = FMath::Sin(fRadAlpha) * FMath::Sin(fRadGamma) + FMath::Cos(fRadAlpha) * FMath::Sin(fRadBeta) * FMath::Cos(fRadGamma);
	
	vecY.X = FMath::Cos(fRadBeta) * FMath::Sin(fRadGamma);
	vecY.Y = FMath::Cos(fRadAlpha) * FMath::Cos(fRadGamma) + FMath::Sin(fRadAlpha) * FMath::Sin(fRadBeta) * FMath::Sin(fRadGamma);
	vecY.Z = -FMath::Sin(fRadAlpha) * FMath::Cos(fRadGamma) + FMath::Cos(fRadAlpha) * FMath::Sin(fRadBeta) * FMath::Sin(fRadGamma);
	
	vecZ.X = -FMath::Sin(fRadBeta);
	vecZ.Y = FMath::Sin(fRadAlpha) * FMath::Cos(fRadBeta);
	vecZ.Z = FMath::Cos(fRadAlpha) * FMath::Cos(fRadBeta);

	vecW.X = 0;
	vecW.Y = 0;
	vecW.Z = 0;

	FMatrix matrix(vecX, vecY, vecZ, vecW);

	FVector vecWorld = matrix.TransformPosition(vecLocal);

	UE_LOG(LogClass, Log, TEXT("vecLocal: (%f, %f, %f)"), vecLocal.X, vecLocal.Y, vecLocal.Z);
	UE_LOG(LogClass, Log, TEXT("vecWorld: (%f, %f, %f)"), vecWorld.X, vecWorld.Y, vecWorld.Z);
	
	CurrentVelocity = CurrentVelocity + vecWorld * 980 * 0.01f;
	CurrentPosition = CurrentPosition + CurrentVelocity * 0.01f;
	*/
}

void ActorPose::UpdateTransform(FTransform trans)
{
	Poco::ScopedLock<Poco::Mutex>	lock(Mutex);

	CurrentPosition = trans.GetLocation();
	CurrentRotation = trans.Rotator();
}

FVector ActorPose::GetLocation()
{
	Poco::ScopedLock<Poco::Mutex>	lock(Mutex);

	return CurrentPosition;
}

FRotator ActorPose::GetRotation()
{
	return CurrentRotation;
}

void ActorPose::Clear()
{
	CurrentVelocity = FVector(0, 0, 0);

	CurrentPosition = FVector(0, 0, 0);
}